/**
 * @file HybridAStarPlanner.cpp
 * @brief 混合A*算法路径规划器实现
 */

#include "planners/HybridAStarPlanner.h"
#include <queue>
#include <unordered_set>
#include <chrono>

namespace PathPlanning
{
// 角度归一化函数
static double normalizeAngle(double angle)
{
    while (angle > M_PI)
        angle -= 2.0 * M_PI;
    while (angle < -M_PI)
        angle += 2.0 * M_PI;
    return angle;
}

HybridAStarPlanner::HybridAStarPlanner(std::shared_ptr<Map> map) : PlannerBase(map)
{
    config_ = std::make_shared<HybridAStarConfig>();
}

PlanningResult HybridAStarPlanner::plan(const Point& start, const Point& goal)
{
    PlanningResult result;
    auto           startTime = std::chrono::high_resolution_clock::now();

    if (!validateStartGoal(start, goal))
    {
        result.message = "Invalid start or goal position";
        return result;
    }

    auto hybridConfig = std::static_pointer_cast<HybridAStarConfig>(config_);

    // 转换为位姿 (默认朝向为0)
    Pose startPose(start.x, start.y, 0);
    Pose goalPose(goal.x, goal.y, 0);

    // 初始化数据结构
    std::priority_queue<std::shared_ptr<HybridAStarNode>, std::vector<std::shared_ptr<HybridAStarNode>>,
                        HybridAStarNodeCompare>
        openSet;

    std::unordered_map<HybridState, std::shared_ptr<HybridAStarNode>, HybridStateHash> allNodes;
    std::unordered_set<HybridState, HybridStateHash>                                   closedSet;

    // 生成运动基元
    auto motionPrimitives = generateMotionPrimitives();

    // 创建起点节点
    auto startNode   = std::make_shared<HybridAStarNode>(startPose);
    startNode->gCost = 0.0;
    startNode->fCost = calculateHeuristic(startNode.get(), goal);

    openSet.push(startNode);
    allNodes[discretizeState(startPose)] = startNode;

    int       iterations    = 0;
    const int maxIterations = 50000;

    while (!openSet.empty() && iterations < maxIterations)
    {
        iterations++;

        auto currentNode = openSet.top();
        openSet.pop();

        HybridState currentState = discretizeState(currentNode->pose);

        if (closedSet.find(currentState) != closedSet.end())
        {
            continue;
        }

        closedSet.insert(currentState);
        result.exploredNodes.push_back(currentNode->pose);

        // 检查是否到达目标
        if (isGoalReached(currentNode.get(), goal))
        {
            result.success = true;
            result.path    = reconstructPath(currentNode);
            break;
        }

        // 可视化回调
        if (vizCallback_ && iterations % 100 == 0)
        {
            result.iterations = iterations;
            vizCallback_(result);
        }

        // 扩展节点
        for (const auto& control : motionPrimitives)
        {
            auto neighbor = applyMotionModel(currentNode.get(), control);

            // 碰撞检测
            if (!isCollisionFree(currentNode.get(), neighbor.get()))
            {
                continue;
            }

            // 计算代价
            double movementCost = euclideanDistance(currentNode->position, neighbor->position);

            // 添加转向惩罚
            if (std::abs(control.steeringAngle) > 0.1)
            {
                movementCost += hybridConfig->turningPenalty;
            }

            // 添加换向惩罚
            if (currentNode->direction != neighbor->direction)
            {
                movementCost += hybridConfig->reversingPenalty;
            }

            neighbor->gCost = currentNode->gCost + movementCost;
            neighbor->fCost = neighbor->gCost + calculateHeuristic(neighbor.get(), goal);

            HybridState neighborState = discretizeState(neighbor->pose);

            auto neighborIt = allNodes.find(neighborState);
            if (neighborIt == allNodes.end())
            {
                // 新节点
                neighbor->parent = currentNode;
                openSet.push(neighbor);
                allNodes[neighborState] = neighbor;
            }
            else
            {
                // 已有节点，检查是否需要更新
                auto existingNode = neighborIt->second;
                if (neighbor->gCost < existingNode->gCost)
                {
                    existingNode->gCost  = neighbor->gCost;
                    existingNode->fCost  = neighbor->fCost;
                    existingNode->parent = currentNode;
                    existingNode->motion = control;

                    openSet.push(existingNode);
                }
            }
        }
    }

    auto endTime        = std::chrono::high_resolution_clock::now();
    result.planningTime = std::chrono::duration<double>(endTime - startTime).count();
    result.iterations   = iterations;

    if (!result.success)
    {
        result.message = "Path not found within iteration limit";
    }
    else
    {
        // 计算路径长度
        for (size_t i = 1; i < result.path.size(); ++i)
        {
            result.pathLength += euclideanDistance(result.path[i - 1].toPoint(), result.path[i].toPoint());
        }
        result.message = "Path found successfully";
    }

    return result;
}

std::vector<Control> HybridAStarPlanner::generateMotionPrimitives()
{
    auto hybridConfig = std::static_pointer_cast<HybridAStarConfig>(config_);

    std::vector<Control> primitives;
    std::vector<double>  steeringAngles = {
        -hybridConfig->maxSteeringAngle, // 右转
        0.0,                             // 直行
        hybridConfig->maxSteeringAngle   // 左转
    };

    for (double angle : steeringAngles)
    {
        primitives.emplace_back(angle, hybridConfig->stepSize, true);  // 前进
        primitives.emplace_back(angle, hybridConfig->stepSize, false); // 后退
    }

    return primitives;
}

std::shared_ptr<HybridAStarNode> HybridAStarPlanner::applyMotionModel(const HybridAStarNode* current,
                                                                      const Control&         control)
{
    auto hybridConfig = std::static_pointer_cast<HybridAStarConfig>(config_);
    auto newNode      = std::make_shared<HybridAStarNode>(current->pose);

    newNode->parent = std::const_pointer_cast<HybridAStarNode>(
        std::shared_ptr<const HybridAStarNode>(current, [](const HybridAStarNode*) {}));
    newNode->direction = control.forward;
    newNode->motion    = control;

    double delta = control.steeringAngle;
    double step  = control.stepLength;

    if (std::abs(delta) < 1e-5)
    {
        // 直行情况
        newNode->pose.x     = current->pose.x + step * cos(current->pose.theta) * (control.forward ? 1 : -1);
        newNode->pose.y     = current->pose.y + step * sin(current->pose.theta) * (control.forward ? 1 : -1);
        newNode->pose.theta = current->pose.theta;
    }
    else
    {
        // 转弯情况 - 自行车模型
        double turningRadius = hybridConfig->wheelbase / tan(delta);
        double angularChange = step / turningRadius * (control.forward ? 1 : -1);

        // 计算转弯中心
        double cx = current->pose.x - sin(current->pose.theta) * turningRadius;
        double cy = current->pose.y + cos(current->pose.theta) * turningRadius;

        newNode->pose.theta = normalizeAngle(current->pose.theta + angularChange);
        newNode->pose.x     = cx + sin(newNode->pose.theta) * turningRadius;
        newNode->pose.y     = cy - cos(newNode->pose.theta) * turningRadius;
    }

    newNode->position = Point(newNode->pose.x, newNode->pose.y);
    return newNode;
}

bool HybridAStarPlanner::isCollisionFree(const HybridAStarNode* from, const HybridAStarNode* to)
{
    // 简化碰撞检测：检查路径上的多个点
    const int numSamples = 10;
    for (int i = 0; i <= numSamples; ++i)
    {
        double t = static_cast<double>(i) / numSamples;

        Pose intermediate;
        intermediate.x = from->pose.x + t * (to->pose.x - from->pose.x);
        intermediate.y = from->pose.y + t * (to->pose.y - from->pose.y);

        // 简化：只检查中心点
        if (map_->isObstacle(intermediate.x, intermediate.y))
        {
            return false;
        }
    }
    return true;
}

double HybridAStarPlanner::calculateHeuristic(const HybridAStarNode* node, const Point& goal)
{
    // 使用欧几里得距离作为启发式函数
    // 在实际实现中，可以使用Reeds-Shepp曲线长度作为更准确的启发式
    return euclideanDistance(node->position, goal);
}

HybridState HybridAStarPlanner::discretizeState(const Pose& pose)
{
    auto hybridConfig = std::static_pointer_cast<HybridAStarConfig>(config_);

    GridCell gridCell        = map_->worldToGrid(pose.x, pose.y);
    double   normalizedTheta = normalizeAngle(pose.theta);
    int      thetaBin        = static_cast<int>((normalizedTheta + M_PI) / (2 * M_PI) * hybridConfig->numThetaBins)
                   % hybridConfig->numThetaBins;

    return HybridState(gridCell.x, gridCell.y, thetaBin);
}

bool HybridAStarPlanner::isGoalReached(const HybridAStarNode* node, const Point& goal)
{
    auto hybridConfig = std::static_pointer_cast<HybridAStarConfig>(config_);

    double posError = euclideanDistance(node->position, goal);
    return posError < hybridConfig->goalPositionTolerance;
}

std::vector<Pose> HybridAStarPlanner::reconstructPath(const std::shared_ptr<HybridAStarNode>& endNode)
{
    std::vector<Pose> path;
    auto              currentNode = endNode;

    while (currentNode != nullptr)
    {
        path.push_back(currentNode->pose);
        if (currentNode->parent)
        {
            currentNode = std::static_pointer_cast<HybridAStarNode>(currentNode->parent);
        }
        else
        {
            break;
        }
    }

    std::reverse(path.begin(), path.end());
    return smoothPath(path);
}

} // namespace PathPlanning